#!/usr/bin/env python3

import os
import rospy
from rospy.msg import AnyMsg
import subprocess
import signal
import time
import airsim
import numpy as np
import quaternion
from std_msgs.msg import Int32, String, Bool
import json
from AirsimPlyGenetor import AirsimPLYGenerator
from tqdm import tqdm
from geometry_msgs.msg import Pose, PoseStamped
from scipy.spatial.transform import Rotation as R


class ManagerNode:
    def __init__(self, args):
        # Initialize parameters from args
        self.cast_until_time = rospy.Time.now().to_sec() - 1.0
        self.is_recording = False
        self.start_is_trigger = False
        self.lap_count = 0
        self.last_pos = None
        self.record_subpross = None
        self.video_path = None
        airsim_ip = self.get_local_host_ip(args)
        self.client = airsim.MultirotorClient(airsim_ip)
        try:
            self.client.confirmConnection()
        except:
            print("AirSim connection failed. Please ensure AirSim is running.")
        self.window_title = self.get_window_title(args)
        self.base_path = args.base_path
        self.record_output_dir = self.get_abs_path(
            args.record_output_dir, self.base_path
        )
        self.current_objects = set()
        self.tags_object = dict()
        self.timer_duration = args.timer_duration
        self.min_overlap_dist = args.min_overlap_dist
        self.start_trigger_box = args.start_trigger_box
        self.end_trigger_box = args.end_trigger_box
        self.scene_config_file = args.scene_config_file
        self.auto_reset_airsim = args.auto_reset_airsim
        self.reset_requested = False
        minx, maxx, miny, maxy, minz, maxz = self.get_scene_range(args)
        self.ply_generator = AirsimPLYGenerator(
            self.client,
            min_x=minx,
            max_x=maxx,
            min_y=miny,
            max_y=maxy,
            min_z=minz,
            max_z=maxz,
        )
        self.get_ply_cb = None
        self.ply_path = None
        self.reset_cb = None
        self.scene_config = None
        self.rng = np.random.default_rng(42)
        self.get_original_obj_pose()
        rospy.Subscriber(args.rec_trigger_topic, AnyMsg,
                         self.rec_trigger_callback)
        rospy.Subscriber(args.reset_topic, AnyMsg, self.reset)
        self.actor_publishers = {}
        self.actor_names = args.actors_to_pub
        for actor in self.actor_names:
            topic = f"/auto_test/{actor}"
            self.actor_publishers[actor] = rospy.Publisher(
                topic, PoseStamped, queue_size=1
            )
        self.drone_pose_pub = rospy.Publisher(
            "/auto_test/drone", PoseStamped, queue_size=1
        )
        self.lap_count_pub = rospy.Publisher(
            args.lap_count_topic, Int32, queue_size=1)
        self.video_names_pub = rospy.Publisher(
            "/video_names", String, queue_size=1)
        self.collision_pub = rospy.Publisher(
            args.collision_topic, Bool, queue_size=1)
        self.timer_tick = 0
        rospy.Timer(rospy.Duration(self.timer_duration), self.timer_callback)

    def reset_rng_seed(self, seed=42):
        self.rng = np.random.default_rng(seed)

    def get_scene_range(self, args):
        minx = args.ply_gen_xrange[0]
        maxx = args.ply_gen_xrange[1]
        miny = args.ply_gen_yrange[0]
        maxy = args.ply_gen_yrange[1]
        minz = args.ply_gen_zrange[0]
        maxz = args.ply_gen_zrange[1]
        return minx, maxx, miny, maxy, minz, maxz

    def get_original_obj_pose(self):
        self.get_current_objects()
        if self.scene_config_file is None:
            return
        self.original_obj_pose = {}
        scene_config_file = self.get_abs_path(
            self.scene_config_file, self.base_path)
        with open(scene_config_file, "r") as f:
            self.scene_config = json.load(f)
            if "objects" in self.scene_config:
                for obj in self.scene_config["objects"]:
                    obj_name = obj["name"]
                    if obj_name not in self.current_objects:
                        continue
                    obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
                    obj_pose = self.client.simGetObjectPose(obj_name)
                    obj_scale = self.client.simGetObjectScale(obj_name)
                    self.original_obj_pose[obj_name] = (obj_pose, obj_scale)
            if "constraints" in self.scene_config:
                for constraint in self.scene_config["constraints"]:
                    if "obj1" in constraint:
                        obj1 = constraint["obj1"]
                        if obj1 not in self.current_objects:
                            continue
                        obj1 = self.tags_object[obj1] if obj1 in self.tags_object else obj1
                        obj1_pose = self.client.simGetObjectPose(obj1)
                        obj1_scale = self.client.simGetObjectScale(obj1)
                        self.original_obj_pose[obj1] = (obj1_pose, obj1_scale)
                    if "obj2" in constraint:
                        obj2 = constraint["obj2"]
                        if obj2 not in self.current_objects:
                            continue
                        obj2 = self.tags_object[obj2] if obj2 in self.tags_object else obj2
                        obj2_pose = self.client.simGetObjectPose(obj2)
                        obj2_scale = self.client.simGetObjectScale(obj2)
                        self.original_obj_pose[obj2] = (obj2_pose, obj2_scale)
            if "swarms" in self.scene_config:
                for swarm in self.scene_config["swarms"]:
                    for obj_name in swarm["objects"]:
                        if obj_name not in self.current_objects:
                            continue
                        obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
                        obj_pose = self.client.simGetObjectPose(obj_name)
                        obj_scale = self.client.simGetObjectScale(obj_name)
                        self.original_obj_pose[obj_name] = (
                            obj_pose, obj_scale)

    def set_request(self, request_type, cb, data):
        if request_type == "generate_ply":
            self.get_ply_cb = cb
            self.ply_path = data["save_path"]
        elif request_type == "reset":
            self.reset_requested = True
            self.reset_cb = cb
        elif request_type == "reset_random_seed":
            seed = data.get("seed", 42)
            self.reset_rng_seed(seed)
            if cb is not None:
                cb()
    def reset(self, msg):
        self.reset_requested = True

    def get_origin_pose(self, obj_name):
        if obj_name not in self.current_objects:
            return None, None
        obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
        return self.original_obj_pose[obj_name]

    def reset_scene(self):
        # Auto reset the AirSim client if enabled
        if self.auto_reset_airsim:
            self.client.reset()

        self.reset_requested = False
        self.lap_count = 0

        # Return early if there's no scene configuration
        if self.scene_config is None:
            return
        operations = []
        if self.scene_config.get("swarms"):
            for swarm in self.scene_config["swarms"]:
                rand_rel_pos = self.get_random_position(swarm)
                rand_rel_rot = self.get_random_rotation(swarm)
                for obj_name in swarm["objects"]:
                    if obj_name not in self.current_objects:
                        print(f"Object {obj_name} not found in scene.")
                        continue
                    obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
                    original_obj_pose, original_obj_scale = self.get_origin_pose(
                        obj_name)
                    if original_obj_pose is None:
                        continue
                    current_ori = self.client.simGetObjectPose(
                        obj_name).orientation
                    new_obj_pose = airsim.Pose(
                        original_obj_pose.position + rand_rel_pos,
                        original_obj_pose.orientation * rand_rel_rot,
                    )
                    if "priority" in swarm:
                        priority = swarm["priority"]
                    else:
                        priority = 0
                    keep_position = ("rand_pos" not in swarm)
                    keep_orientation = ("rand_rot" not in swarm)
                    operations.append(
                        {"operation": "set_pose", "priority": priority, "obj_name": obj_name, "pose": new_obj_pose, "keep_position": keep_position, "keep_orientation": keep_orientation})

        # Handle scene objects
        if self.scene_config.get("objects"):
            for obj in self.scene_config["objects"]:
                obj_name = obj["name"]
                if obj_name not in self.current_objects:
                    print(f"Object {obj_name} not found in scene.")
                    continue
                obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
                rand_rel_pos = self.get_random_position(obj,  obj_name)
                rand_rel_rot = self.get_random_rotation(
                    obj,  obj_name)  # b -> o
                rand_scale = self.get_random_scale(obj, obj_name)
                if rand_scale is None:
                    continue
                original_obj_pose, original_obj_scale = self.get_origin_pose(
                    obj_name)  # o -> w
                if original_obj_pose is None:
                    continue
                new_obj_pose = airsim.Pose(
                    original_obj_pose.position + rand_rel_pos,
                    original_obj_pose.orientation * rand_rel_rot,
                )
                if "priority" in obj:
                    priority = obj["priority"]
                else:
                    priority = 0
                keep_position = ("rand_pos" not in obj)
                keep_orientation = ("rand_rot" not in obj)
                operations.append({"operation": "set_pose", "priority": priority,
                                  "obj_name": obj_name, "pose": new_obj_pose, "keep_position": keep_position, "keep_orientation": keep_orientation})
                operations.append(
                    {"operation": "set_scale", "priority": priority, "obj_name": obj_name, "scale": rand_scale, "keep_position": True, "keep_orientation": True})

        # Handle scene constraints
        if "constraints" in self.scene_config:
            for constraint in self.scene_config["constraints"]:
                if "type" not in constraint:
                    continue
                if constraint["type"] == "distance":
                    operation = self.apply_distance_constraint(constraint)
                elif constraint["type"] == "angle":
                    operation = self.apply_angle_constraint(constraint)
                if operation is not None:
                    operations.append(operation)
        operations = sorted(
            operations, key=lambda x: x["priority"], reverse=True)
        for operation in operations:
            if operation["operation"] == "set_pose":
                new_position = operation["pose"].position
                if operation.get("keep_position", False):
                    new_position = self.client.simGetObjectPose(
                        operation["obj_name"]).position
                new_orientation = operation["pose"].orientation
                if operation.get("keep_orientation", False):
                    new_orientation = self.client.simGetObjectPose(
                        operation["obj_name"]).orientation
                self.client.simSetObjectPose(
                    operation["obj_name"],
                    airsim.Pose(new_position, new_orientation),
                )
            elif operation["operation"] == "set_scale":
                self.client.simSetObjectScale(
                    operation["obj_name"], operation["scale"],
                )
            elif operation["operation"] == "set_dpose":
                ori_obj_pose = self.client.simGetObjectPose(
                    operation["ori_obj"])
                new_position = ori_obj_pose.position + operation["dpos"]
                obj_pose = self.client.simGetObjectPose(
                    operation["obj_name"])
                new_orientation = ori_obj_pose.orientation
                self.client.simSetObjectPose(
                    operation["obj_name"],
                    airsim.Pose(new_position, obj_pose.orientation),
                )

    def get_random_position(self, obj, obj_name=None):
        if "rand_pos" in obj:
            return airsim.Vector3r(
                self.rng.uniform(*obj["rand_pos"][0]),
                self.rng.uniform(*obj["rand_pos"][1]),
                self.rng.uniform(*obj["rand_pos"][2]),
            )
        elif obj_name is not None and obj_name in self.current_objects:
            obj_name = self.tags_object[obj_name] if obj_name in self.tags_object else obj_name
            current_pos = self.client.simGetObjectPose(obj_name).position
            ori_pos = self.get_origin_pose(obj_name)[0].position
            if current_pos is not None:
                return airsim.Vector3r(
                    current_pos.x_val - ori_pos.x_val, current_pos.y_val -
                    ori_pos.y_val, current_pos.z_val - ori_pos.z_val
                )
            else:
                print(f"Object {obj['name']} has no position.")
        return airsim.Vector3r(0, 0, 0)

    def get_random_rotation(self, obj, obj_name=None):
        if "rand_rot" in obj:
            return airsim.to_quaternion(
                self.rng.uniform(*obj["rand_rot"][1]) * np.pi / 180,
                self.rng.uniform(*obj["rand_rot"][0]) * np.pi / 180,
                self.rng.uniform(*obj["rand_rot"][2]) * np.pi / 180,
            )  # pitch, roll, yaw. Da sb.
        return airsim.to_quaternion(0, 0, 0)

    def get_random_scale(self, obj, obj_name):
        original_obj_scale = self.get_origin_pose(obj_name)[1]
        if original_obj_scale is None:
            return None
        if "rand_scale" in obj:
            if "maintain_aspect_ratio" in obj:
                scale = self.rng.uniform(*obj["rand_scale"][0])
                return airsim.Vector3r(
                    scale * original_obj_scale.x_val,
                    scale * original_obj_scale.y_val,
                    scale * original_obj_scale.z_val,
                )
            else:
                return airsim.Vector3r(
                    self.rng.uniform(*obj["rand_scale"]
                                      [0]) * original_obj_scale.x_val,
                    self.rng.uniform(*obj["rand_scale"]
                                      [1]) * original_obj_scale.y_val,
                    self.rng.uniform(*obj["rand_scale"]
                                      [2]) * original_obj_scale.z_val,
                )
        return original_obj_scale

    def apply_distance_constraint(self, constraint):
        if (
            "obj1" not in constraint
            or "obj2" not in constraint
            or "value" not in constraint
        ):
            return None

        obj1, obj2 = constraint["obj1"], constraint["obj2"]
        print(f"Applying distance constraint between {obj1} and {obj2}")
        if obj1 not in self.current_objects or obj2 not in self.current_objects:
            print(f"Object {obj1} or {obj2} not found in scene.")
            return None
        obj1 = self.tags_object[obj1] if obj1 in self.tags_object else obj1
        obj2 = self.tags_object[obj2] if obj2 in self.tags_object else obj2
        relative_pos = airsim.Vector3r(*constraint["value"])

        if "priority" in constraint:
            priority = constraint["priority"]
        else:
            priority = 0
        return {"operation": "set_dpose", "priority": priority, "obj_name": obj2,"ori_obj": obj1, "dpos": relative_pos}

    def apply_angle_constraint(self, constraint):
        if (
            "obj1" not in constraint
            or "obj2" not in constraint
            or "value" not in constraint
        ):
            return None
        obj1, obj2 = constraint["obj1"], constraint["obj2"]
        if obj1 not in self.current_objects or obj2 not in self.current_objects:
            return None
        obj1 = self.tags_object[obj1] if obj1 in self.tags_object else obj1
        obj2 = self.tags_object[obj2] if obj2 in self.tags_object else obj2
        euler_angle = airsim.Vector3r(*constraint["value"]) * np.pi / 180
        relative_quat = airsim.to_quaternion(
            euler_angle.y_val, euler_angle.x_val, euler_angle.z_val
        )  # b2 -> b1
        obj1_pose = self.client.simGetObjectPose(obj1)  # b1 -> o1
        obj2_pose = self.client.simGetObjectPose(obj2)  # b2 -> o2
        obj1_original_ori = self.get_origin_pose(
            obj1)[0].orientation  # o1 -> w
        if obj1_original_ori is None:
            return None
        new_obj2_pose = airsim.Pose(
            obj2_pose.position,
            obj1_original_ori * obj1_pose.orientation * relative_quat,
        )  # b2 -> w
        if "priority" in constraint:
            priority = constraint["priority"]
        else:
            priority = 0
        return {"operation": "set_pose", "priority": priority, "obj_name": obj2, "pose": new_obj2_pose}

    def get_window_title(self, args):
        if args.window_title is not None:
            return args.window_title
        window_title = os.path.basename(args.simulator_script).split(".")[0]
        return window_title

    def get_abs_path(self, path, base_path):
        if os.path.isabs(path):
            return path
        """Get absolute path for the given path."""
        return os.path.join(base_path, path)

    def get_local_host_ip(self, args):
        settings_file = self.get_abs_path("settings.json", args.base_path)
        json_file = open(settings_file)
        settings = json.load(json_file)
        json_file.close()
        return settings["LocalHostIp"]

    def record_window(self, output_file, x, y, width, height):
        if not os.path.exists(os.path.dirname(output_file)):
            os.makedirs(os.path.dirname(output_file))

        command = f"ffmpeg -f x11grab -s {width}x{height} -framerate 30 -i :0.0+{x},{y} -c:v libx264 -c:a aac -b:a 192k -f mp4 -pix_fmt yuv420p {output_file}"

        try:
            print(command)
            self.record_subpross = subprocess.Popen(
                command, shell=True, preexec_fn=os.setsid
            )
        except subprocess.CalledProcessError as e:
            print("Recording failed:", e)

    def get_window_position(self, window_title):
        id = self.get_window_id(window_title)
        if id is None:
            return None
        try:
            output = subprocess.check_output("xwininfo -id " + id, shell=True)
        except subprocess.CalledProcessError:
            print("Error: xwininfo not found or not installed.")
            return None
        upper_left_x = None
        upper_left_y = None
        width = None
        height = None
        for line in output.decode("utf-8").splitlines():
            if "Absolute upper-left X:" in line:
                upper_left_x = int(line.split()[3])
            elif "Absolute upper-left Y:" in line:
                upper_left_y = int(line.split()[3])
            elif "Width:" in line:
                width = int(line.split()[1])
            elif "Height:" in line:
                height = int(line.split()[1])
        if (
            upper_left_x is None
            or upper_left_y is None
            or width is None
            or height is None
        ):
            print("Error: Could not get window position.")
            return None
        return (upper_left_x, upper_left_y, width, height)

    def get_window_id(self, window_title):
        try:
            output = subprocess.check_output(
                "xdotool search --name " + window_title, shell=True
            )
        except subprocess.CalledProcessError:
            print("Error: xdotool not found or not installed.")
            return None
        return output.decode("utf-8").strip()

    def pub_actor_pose(self):
        for actor in self.actor_names:
            airsim_pose = self.client.simGetObjectPose(actor)
            if (
                np.isnan(airsim_pose.position.x_val)
                or np.isnan(airsim_pose.position.y_val)
                or np.isnan(airsim_pose.position.z_val)
                or np.isnan(airsim_pose.orientation.w_val)
                or np.isnan(airsim_pose.orientation.x_val)
                or np.isnan(airsim_pose.orientation.y_val)
                or np.isnan(airsim_pose.orientation.z_val)
            ):
                continue
            pose = PoseStamped()
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = airsim_pose.position.x_val
            pose.pose.position.y = airsim_pose.position.y_val
            pose.pose.position.z = airsim_pose.position.z_val
            pose.pose.orientation.w = airsim_pose.orientation.w_val
            pose.pose.orientation.x = airsim_pose.orientation.x_val
            pose.pose.orientation.y = airsim_pose.orientation.y_val
            pose.pose.orientation.z = airsim_pose.orientation.z_val
            self.actor_publishers[actor].publish(pose)

    def pub_drone_pose(self, drone_pos: airsim.Pose):
        msg = PoseStamped()
        msg.header.stamp = rospy.Time.now()
        msg.pose.position.x = drone_pos.position.x_val
        msg.pose.position.y = drone_pos.position.y_val
        msg.pose.position.z = drone_pos.position.z_val
        msg.pose.orientation.x = drone_pos.orientation.x_val
        msg.pose.orientation.y = drone_pos.orientation.y_val
        msg.pose.orientation.z = drone_pos.orientation.z_val
        msg.pose.orientation.w = drone_pos.orientation.w_val
        self.drone_pose_pub.publish(msg)

    def start_record(self):
        region = self.get_window_position(self.window_title)
        if region is not None:
            curtime = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime())
            self.video_path = os.path.join(
                self.record_output_dir, f"{curtime}.mp4")
            self.record_window(
                self.video_path, region[0], region[1], region[2], region[3]
            )

    def stop_record(self):
        os.killpg(os.getpgid(self.record_subpross.pid), signal.SIGINT)
        self.record_subpross.wait()
      
    def rec_trigger_callback(self, msg):
        self.cast_until_time = rospy.Time.now().to_sec() + 1.0

    def get_current_objects(self):
        objects = self.client.simListSceneObjects()
        self.current_objects = set()
        self.tags_object = dict()
        for obj in objects:
            self.current_objects.add(obj)
        try:
            objects_tags = self.client.client.call("simListSceneObjectsTags", '.*')
            for object_tag in objects_tags:
                self.current_objects.add(object_tag[1])
                self.tags_object[object_tag[1]] = object_tag[0]
        except:
            pass
    def timer_callback(self, event):
        self.timer_tick += 1
        self.timer_tick %= 100
        if self.timer_tick % 10 == 0:
            self.get_current_objects()
        connected = self.client.ping()
        if not connected:
            print("AirSim is not connected.")
            return
        time_now = rospy.Time.now().to_sec()
        self.update_collision_state()
        drone_pose = self.client.simGetVehiclePose()
        self.pub_drone_pose(drone_pose)
        drone_pos = drone_pose.position
        drone_pos = np.array(
            [drone_pos.x_val, drone_pos.y_val, drone_pos.z_val])
        self.pub_actor_pose()
        if not self.is_recording and time_now < self.cast_until_time:
            print("Start recording")
            self.is_recording = True
            self.start_is_trigger = False
            self.last_pos = None
            self.lap_count = 0
            self.start_record()
        elif self.is_recording and time_now > self.cast_until_time:
            print("Stop recording")
            self.stop_record()
            self.is_recording = False
        if self.reset_requested:
            self.reset_scene()
            if self.reset_cb is not None:
                reset_cb = self.reset_cb
                self.reset_cb = None
                reset_cb()
        if self.get_ply_cb is not None:
            get_ply_cb = self.get_ply_cb
            save_path = self.ply_path
            self.get_ply_cb = None
            self.ply_path = None
            with tqdm(total=100, desc="Saving PLY") as pbar:
                for process in self.ply_generator.generate(save_path):
                    pbar_len = int(process * 100 - pbar.n)
                    pbar.update(pbar_len)
            get_ply_cb()

        if self.start_trigger_box is not None and self.end_trigger_box is not None:
            # Check if StartTriggerBox is defined and overlap detection should be performed
            if self.start_trigger_box and not self.start_is_trigger:
                if self.box_drone_overlap(self.start_trigger_box, drone_pos):
                    self.start_is_trigger = True
            else:
                # Check if EndTriggerBox is defined and overlap detection should be performed
                if self.end_trigger_box and self.start_is_trigger:
                    if self.box_drone_overlap(self.end_trigger_box, drone_pos):
                        self.lap_count += 1
                        self.start_is_trigger = False
            self.last_pos = drone_pos
            self.lap_count_pub.publish(Int32(self.lap_count))
        if self.video_path is not None:
            self.video_names_pub.publish(String(self.video_path))

    def update_collision_state(self):
        is_collision = self.client.simGetCollisionInfo().has_collided
        self.collision_pub.publish(Bool(is_collision))

    def box_drone_overlap(self, box_name, drone_pos):
        box_pose = self.client.simGetObjectPose(box_name)
        box_pos = np.array(
            [box_pose.position.x_val, box_pose.position.y_val, box_pose.position.z_val]
        )
        box_scale = self.client.simGetObjectScale(box_name)
        box_radius = (
            np.array([box_scale.x_val, box_scale.y_val, box_scale.z_val])
            * self.min_overlap_dist
        )
        box_quat = box_pose.orientation
        box_rot = quaternion.from_float_array(
            [box_quat.w_val, box_quat.x_val, box_quat.y_val, box_quat.z_val]
        )
        box_rot = quaternion.as_rotation_matrix(box_rot)
        return self.drone_is_in_box(drone_pos, box_pos, box_rot, box_radius)

    def drone_is_in_box(self, drone_pos, box_pos, box_rot, box_radius):
        if self.last_pos is None:
            self.last_pos = drone_pos
            return False
        minim_detect_dist = box_radius.min() / 2
        total_dist = np.linalg.norm(drone_pos - self.last_pos)
        if total_dist < 1e-3:
            return False
        detect_dir = (self.last_pos - drone_pos) / total_dist
        disti = 0.0
        while disti < total_dist:
            detect_pos = drone_pos + disti * detect_dir
            detect_pos = np.dot(box_rot.T, detect_pos - box_pos)
            if (
                np.abs(detect_pos[0]) < box_radius[0]
                and np.abs(detect_pos[1]) < box_radius[1]
                and np.abs(detect_pos[2]) < box_radius[2]
            ):
                return True
            disti += minim_detect_dist
        return False

    def stop(self):
        if self.is_recording and self.record_subpross is not None:
            self.stop_record()
        rospy.signal_shutdown("Shutting down")


if __name__ == "__main__":
    time.sleep(2)
    print("Start record")
    rospy.init_node("simulator_manager_node")

    # Define default parameters (these could be adjusted via launch files or parameters)
    import argparse

    parser = argparse.ArgumentParser(
        description="Record UAV activity and trigger events."
    )
    parser.add_argument(
        "--window-title",
        type=str,
        default="new_window",
        help="Title of the window to capture",
    )
    parser.add_argument(
        "--record-output-dir",
        type=str,
        default="record",
        help="Directory to save recorded files",
    )
    parser.add_argument(
        "--imu-topic", type=str, default="/mavros/imu/data", help="IMU data topic"
    )
    parser.add_argument(
        "--circle-count-topic",
        type=str,
        default="/lap_count",
        help="Topic to publish circle count",
    )
    parser.add_argument(
        "--timer-duration", type=float, default=0.1, help="Timer callback duration"
    )
    parser.add_argument(
        "--min-overlap-dist",
        type=float,
        default=0.5,
        help="Minimum distance for detecting drone in box",
    )
    parser.add_argument(
        "--start-trigger-box",
        type=str,
        default=None,
        help="Name of the Start Trigger Box object (optional)",
    )
    parser.add_argument(
        "--end-trigger-box",
        type=str,
        default=None,
        help="Name of the End Trigger Box object (optional)",
    )

    args = parser.parse_args()
    manager_node = ManagerNode(args)

    while not rospy.is_shutdown():
        rospy.spin()
